#ifndef __QBOT_H
#define __QBOT_H

#include "qbot_master.h"
#include "qbot_parser.h"
#include <fstream>

#pragma once

namespace Qbot
{
    SerialPort serial_port("/dev/ttyUSB0", 115200);
    QbotMaster qbot(&serial_port);
    QbotWebParser parser(&qbot);

    boost::thread battery_task;
    boost::thread background_task;

    void background_loop(void)
    {
        std::cout << "Background Thread Start." << std::endl;
        while (qbot.is_alive())
        {
            boost::this_thread::sleep_for(boost::chrono::milliseconds(1));
            if(parser.client_timeout())
            {
                parser.reset_timeout();  // reset timeout cnt value
                if( qbot.get_speed() != 0 )
                    std::cout << "Communication Timeout, RGV Stop Now." << std::endl;
                qbot.set_speed(0);
            }
        }
        std::cout << "Background Thread Exit." << std::endl;
    }

    void battery_loop(void)
    {
        static Json::Value battery_json;
        static Json::CharReaderBuilder json_builder;
        static std::ifstream battery_file;

        JSONCPP_STRING err;
        std::cout << "Battery Reading Thread Start" << std::endl;

        while (qbot.is_alive())
        {
            boost::this_thread::sleep_for(boost::chrono::seconds(1));
            try
            {
                battery_file.open("/home/admin1/log/log.txt");
                if(battery_file.is_open())
                {
                    if (!Json::parseFromStream(json_builder, battery_file, &battery_json, &err))
                    {
                        std::cerr << err << std::endl;
                    }
                    else
                    {
                        qbot.battery_info.BMS.RSOC = battery_json["BMS"]["RSOC"].asDouble();
                        qbot.battery_info.BMS.current = battery_json["BMS"]["current"].asDouble();
                        qbot.battery_info.BMS.voltage = battery_json["BMS"]["voltage"].asDouble();
                        //std::cout << "RSOC: " << battery_info.BMS.RSOC << std::endl;
                        for (int i = 0; i < 6; i++)
                        {
                            qbot.battery_info.DI[i] = battery_json["DI"][i].asBool();
                            qbot.battery_info.DO[i] = battery_json["DO"][i].asBool();
                        }
                    }
                }
            }
            catch (const std::exception &e)
            {
                std::cerr << e.what() << std::endl;
            }
            if (battery_file.is_open())
                battery_file.close();
        }
        std::cout << "Battery Reading Thread Terminate" << std::endl;
    }

    void battery_init(void)
    {
        battery_task = boost::thread(battery_loop);
    }

    void background_init(void)
    {
        background_task = boost::thread(background_loop);
    }

} // namespace Qbot

#endif
